using System.Collections;
using System.Collections.Generic;
using System.Data;
using Unity.VisualScripting;
using UnityEngine;
using RobotSimulator.Basic;
using System.Drawing;
public class IMU : BasicSensor
{
    public Rigidbody rb;
    public IMUData data = new();
    protected override void Start()
    {
        base.Start();
        if(rb == null)
        {
            rb = GetComponentInParent<Rigidbody>();
        }
    }
    private float last_time;
    private Vector3 last_velocity;
    protected override void FixedUpdate()
    {
        float moment_time = Time.time;
        data.angularVelocity = rb.angularVelocity;
        data.rotation = rb.rotation;
        data.velocity = rb.velocity;
        data.acceleration = (rb.velocity - last_velocity) / (moment_time - last_time);
        last_time = Time.time;
        last_velocity = rb.velocity;
        base.FixedUpdate();
    }
    public static Vector3 Euler(Quaternion quaternion)
    {
        float src_pitch = quaternion.eulerAngles.z;
        float src_roll = quaternion.eulerAngles.x;
        float src_yaw = quaternion.eulerAngles.y;
        float pitch = src_pitch > 180 ? 360 - src_pitch : -src_pitch;
        float roll = src_roll > 180 ? 360 - src_roll : -src_roll;
        float yaw = src_yaw > 180 ? 360 - src_yaw : -src_yaw;
        return new(roll, yaw, pitch);
    }
}

public class CmdGetIMUData : CmdGetSensorData
{
    protected override string GetData(IEU ieu)
    {
        IMU imu = (IMU)ieu;
        imu.output_data = JsonUtility.ToJson(imu.data);
        return base.GetData(ieu);
    }
}
public class CmdGetIMUAngularVelocity : CmdGetSensorData
{
    protected override string GetData(IEU ieu)
    {
        IMU imu = (IMU)ieu;
        imu.output_data = JsonUtility.ToJson(imu.data.angularVelocity);
        return base.GetData(ieu);
    }
}
public class CmdGetIMURotationEuler : CmdGetSensorData
{
    protected override string GetData(IEU ieu)
    {
        IMU imu = (IMU)ieu;
       
        imu.output_data = JsonUtility.ToJson(IMU.Euler(imu.data.rotation));
        return base.GetData(ieu);
    }
}
public class IMUData
{
    public Vector3 angularVelocity;
    public Vector3 acceleration;
    public Vector3 velocity;
    public Quaternion rotation;
    public IMUData()
    { 
        angularVelocity = Vector3.zero;
        acceleration = Vector3.zero;
        velocity = Vector3.zero;
        rotation = new Quaternion(0, 0, 0, 0);
    }
}
public class FixAngle
{
    private float last_angle=0;
    private float res_angle=0;
    public float Fix(float angle)
    {
        float da = angle - last_angle;
        if (da > 180)
        {
            da = da - 360;
        }
        else if (da < -180)
        {
            da = 360 + da;
        }
        res_angle += da;
        last_angle = angle;
        return res_angle;
    }
}